% Kalman ETENDU Filter 
clearvars
clf

% V = Vitesse
% W = Rotation

%Si on utilise une vitesse 
dt=0.1;
theta=0;
x=0;
y=0;
% Bruit sur la vitesse
Cv=0;
% Bruit sur rotation
Cw=0;
% z sera la vrai mesure
z=0;

%etat [x y theta]
X = [x y theta]';
P = zeros(3,3);

%covariance 
F = [1 0 -V*dt*sin(theta); 0 1 V*dt*sin(theta); 0 0 1];

%jacobienne 
G = [dt*cos(theta) 0 ; dt*sin(theta) 0; 0 dt];



u = [V W]';
Xb= X(1);
Yb= X(2);

 % ====== Propagation =========

 	X(1) = X(1) + V*cos(X(3))*dt;
 	X(2) = X(2) + V*cos(X(3))*dt;
 	X(3) = X(3) + omega*dt;

 	zhat = sqrt((X(1)-Xb)^2 + (X(2)-Yb)^2);

% CALCUL DES MATRICE JACOBIENNE
 	F = [1 0 -V*sin(X(3))*dt; 0 1 V*cos(X(3))*dt; 0 0 1];
 	G = [dt*cos(X(3)) dt*sin(X(3)) 0 ; 0 0 dt]';
 	H = [(X(1)-Xb)/zhat (X(2)-Yb)/zhat 0];

 	P = F*P*F' + G*Cv*G';

	X = F*X + G*U;
	P = F*P*F' + G*Cv*G';

% MISE A JOUR 
 K=P*H'/(H*P*H'+Cw);
 r = (z-zhat);
 X = X + K*r;
 P = (eye(3)-K*H)*P;